import serial
import logging
import time
# 配置日志 - 设置为 INFO 级别
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
)
logger = logging.getLogger(__name__)


class RoboCtrl:
    def __init__(self, port, baudrate=115200, timeout=1):
        """
        初始化机器人控制器
        
        Args:
            port: 串口端口号（如 'COM3' 或 '/dev/ttyUSB0'）
            baudrate: 波特率，默认115200
            timeout: 超时时间，默认1秒
        """
        self.port = port
        try:
            logger.info(f"初始化尝试连接: {port}")
            self.ser = serial.Serial(port, baudrate, timeout=timeout)
            self.port = port
            logger.info(f"成功连接到端口 {port}")
        except Exception as e:
            logger.error(f"连接失败: {e}")
            self.ser = None

    def run_guidewire(self, speed):
        """
        导丝推送/撤回控制
        
        Args:
            speed: 速度值，正数为推送，负数为撤回
        """
        if self.ser is None:
            logger.warning("串口未连接")
            return False
        speed = -speed * 2
        try:
            cmd = f":3 {speed:.2f}\r\n"
            self.ser.write(cmd.encode())
            logger.info(f"导丝命令: {cmd.strip()}")
            return True
        except Exception as e:
            logger.error(f"发送导丝命令失败: {e}")
            return False
    
    def rot_guidewire(self, speed):
        """
        导丝旋转控制
        
        Args:
            speed: 速度值，正数为顺时针，负数为逆时针
        """
        if self.ser is None:
            logger.warning("串口未连接")
            return False
        speed = speed*60
        try:
            cmd = f":1 {speed:.2f}\r\n"
            self.ser.write(cmd.encode())
            logger.info(f"导丝旋转命令: {cmd.strip()}")
            return True
        except Exception as e:
            logger.error(f"发送导丝旋转命令失败: {e}")
            return False
    
    def run_cath(self, speed):
        """
        导管推送/撤回控制
        
        Args:
            speed: 速度值，正数为推送，负数为撤回
        """
        if self.ser is None:
            logger.warning("串口未连接")
            return False
        speed = speed * 100
        try:
            cmd = f":0 {speed:.2f}\r\n"
            self.ser.write(cmd.encode())
            logger.info(f"导管命令: {cmd.strip()}")
            return True
        except Exception as e:
            logger.error(f"发送导管命令失败: {e}")
            return False
    
    def ping(self):
        """
        检测连接状态
        """

        try:
            if self.ser is None:
                logger.warning(f"串口尝试连接中 {self.port}")
                self.ser = serial.Serial(self.port, 115200, timeout=1)
                logger.info(f"成功连接到端口 {self.port}")
            self.ser.write(b":P\r\n")
            logger.info("发送Ping命令")
            return True
        
        except Exception as e:
            logger.error(f"Ping命令失败: {e}")
            return False
        
       
    
    def close(self):
        """关闭串口连接"""
        if self.ser:
            self.ser.close()
            logger.info("串口连接已关闭")


if __name__ == "__main__":
    # 创建机器人控制器对象
    robot = RoboCtrl('COM121')
    # 测试推送/撤回
    robot.rot_guidewire(5)  # 推送 0.5m/s
    time.sleep(1)
    robot.rot_guidewire(-5)  # 撤回 0.5m/s
